clc; clear; 
close all;

global dt
quadrotor_parameters;

t = 0 : dt : Tf;
N = Tf/dt + 1;
x = x0; % 初始位置
X = []; % 存放绘图数据
x_2dot = zeros(6,1); 
% x_2dot_1 = zeros(12,1); 
Uxy = zeros(2,N);
Uxy2 = [0;0];
U_save = zeros(N,4);
for i = 1:N

    [U, Uxy(:,i+1)] = quadrotor_controller(t(i),x,Uxy(:,i),Uxy2);
    Uxy2 = Uxy(:,i);  % 将第i列的数据保存在Uxy2中
    
    k1 = quadrotor_plant(x,           U);
    k2 = quadrotor_plant(x + dt/2*k1, U);
    k3 = quadrotor_plant(x + dt/2*k2, U);
    k4 = quadrotor_plant(x + dt*k3,   U);

    x = x + dt/6*(k1 + 2*k2 + 2*k3 +k4);
%     x_2dot_1 = quadrotor_plant(x,U);
    X = [X;x'];
    U_save(i,:) = U;
end

%% Plot Results
figure
quatrotor_plot(t,U_save(:,1))

figure
quatrotor_plot(t,U_save(:,2))
figure
quatrotor_plot(t,U_save(:,3))
figure
quatrotor_plot(t,U_save(:,4))

figure
% Position 
quatrotor_plot(t, X(:,9),'-b'); 
hold on
quatrotor_plot(t, X(:,11),'-r');
quatrotor_plot(t, X(:,7), '-g')
grid on;
title('Position Response');
legend('X','Y','Z');

figure
% Attitude 
quatrotor_plot(t, rad2deg(X(:,1)), '-b'); hold on;
quatrotor_plot(t, rad2deg(X(:,3)), '-r'); grid on;
title('PHI & THETA');
legend('\phi','\theta');

figure
quatrotor_plot(t, rad2deg(X(:,5)), '-g');
title('PSI'); 
legend('\psi'); 

figure 
plot3(X(:,9),X(:,11),X(:,7),'Color','#0000FF','Linewidth',3);
title('line in 3-D Space');
xlabel('X');
ylabel('Y');
zlabel('Z');hold on
grid on;

A = 2;
w = 2*pi/40;
% w = 2*pi/20;
% w = 2*pi/100;
a = deg2rad(5);
Xd = A*cos(w*t);
Yd = A*sin(w*t);
Zd = t/10;
plot3(Xd,Yd,Zd,'Color','#FF0000','Linewidth',1.5);
grid on;